#ifndef MATRIX_H
#define MATRIX_H

/* -------------------------------------------------------------------------- *
 * File: Matrix.h                                                             *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "CData.h"
#include "Utils.h"

#include <iostream>
#include <cstdlib>

namespace RSIM{

/**
Matrix_ class: The indexing is 0 based. Matlab style access operations are valid. 
That is M(i,j) would access the element from i^th row and j^th column. The default 
constructor creates a null matrix. Space must be allocated calling setDimension() 
before using a null matrix. 
*/

template<class T>class Vector_;

template<class T>
class Matrix_:public cData<T>{
        protected:
                int row_, col_;
        public:
                
                /** Null matrix */
                Matrix_();
                
                /** Deep copy */
                Matrix_(const Matrix_<T>& M);
                
                Matrix_(const Matrix_<T>& M, const int& row, const int& col);
                
                /** Deep copy */
                explicit Matrix_(const Vector_<T>& V);
                
                /** Allocate space of specified dimension */
                Matrix_(const int& i,const int& j);
                
                /** Allocate space and copy values in dataPtr. No check on validity of dataPtr 	*/
                Matrix_(const int& row,const int& col,const T *dataPtr);
                
                /** Allocate space and set each value to val */
                Matrix_(const int& row,const int& col,const T& val);
                
                /** Delete allocated space. Set matrix to NULL */
                void clear();

                /** @return Number of rows */
                int getNRow()const;
                
                /** @return Number of rows */
                int dim1()const;
                
                /** @return Number of columns */
                int getNCol()const;
                
                /** @return Number of columns */
                int dim2()const;
                
                /** @return Number of elements in the matrix */
                int getNElements()const;
                
                /** Allocates space. This is to be used if the Matrix_ object was set using the constructor Matrix_() */
                void setDimension(const int& NumRow,const int& NumCol);
                
                /** Allocates space. This is to be used if the Matrix_ object was set using the constructor Matrix_() */
                void resize(const int& NumRow, const int& NumCol);
                                                                
                /**		
                Space must be allocated before using any of the functions below. 
                No checks performed to ensure this or the validity of the input 
                indices. The burden is on the user. 
                */
                
                /** @return M(i,j) */
                const T& getElement(const int& i,const int& j)const;
                
                /** @return i^{th} element of the contiguous block of memory */
                const T& getElement(const int& i)const;
                
                /** M(i,j) = v */
                void setElement(const int& i,const int& j,const T& v);

                /** set i^th element of the contiguous block of memory */
                void setElement(const int& i,const T& v);
                
                /** Set all elements to zero*/
                void Zeros();
                
                /** Turn matrix to Diagonal with each diagonal element set to val */
                void Diagonal(const T& val);
                
                /** Set Matrix_ to Identity */
                void Identity();
                
                /** Print rows and columns of this matrix */
                void printDimensions();
                
                /** returns a square matrix of size n x n. The rows and columns of this
                matrix must be greater than n.*/
                Matrix_<T> extractSquareBlockOfSize(const int& n)const;
                
                /** Returns a new matrix with the same number of columns but with 
                specified number of rows*/
                Matrix_<T> extractNRows(const int& n)const;
                
                Matrix_<T> operator+(const Matrix_<T>& Mat)const;
                
                Matrix_<T> operator-(const Matrix_<T>& M)const;
                
                /** matrix x scalar */
                Matrix_<T> operator*(const T& val)const;
                        
                Vector_<T> operator+(const Vector_<T>& V)const;
                
                Vector_<T> operator-(const Vector_<T>& V)const;
                
                /**  access element M(i,j) */
                T& operator () (const int& i, const int& j);
                
                /**  access i^th element from the contiguous block of memory */
                T& operator () (const int& i);
                
                /** matrix x matrix */
                Matrix_<T> operator*(const Matrix_<T>& M)const;
                
                /** matrix * vector */
                Matrix_<T> operator*(const Vector_<T>& V)const;
                
                /** Transpose of a Matrix_ */
                Matrix_<T> operator~(void) const;	
                
                /** Returns the negative of this matrix*/
                Matrix_<T> operator-(void)const;
                
                /** Deep copy */
                void operator = (const Matrix_<T>&);
                
                /** Deep copy */
                void operator = (const Vector_<T>&);

                /** Operator included to make it compatible with TNT and JAMA
                Linear Algebra packages. See documentation for TNT::Array2D
                */
                T* operator[](const int& i);
                
                /** Operator included to make it compatible with TNT and JAMA
                Linear Algebra packages. See documentation for TNT::Array2D		
                */
                const T* operator[](const int& i)const;
                
                template <class U> 
                friend std::ostream& operator<<(ostream& func,const RSIM::Matrix_<U>& M);
                
                template <class U>
                friend Matrix_<U> operator*(const double& val,const Matrix_<U>& Mat);
                
                template <class U>
                friend std::istream& operator>>(istream& func,Matrix_<U>& M);
};

///////////////////////////////////////////////////////////////
template <class T>
Matrix_<T> Matrix_<T>::extractNRows(const int& n)const{        
        #ifdef RSIM_SAFE
        Utils::rsim_assert(n > 0,
                           "Matrix_<T>::extractNRows(const int& n), n must be greater than zero");        
                           
        Utils::rsim_assert(this->row_ >= n,
                           "Matrix_<T>::extractNRows(const int& n), n is too large");
        #endif
        
        Matrix_<T> op(n,this->col_);
        double *mat = op.wPtr();
        int tot = n*this->col_;
        for(int i=0;i<tot;mat[i]=this->data_[i++]);
        return op;
}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T> Matrix_<T>::extractSquareBlockOfSize(const int& n)const{
        #ifdef RSIM_SAFE
        Utils::rsim_assert(this->row_ > n,
                           "Matrix_<T>::extractSquareBlockOfSize(const int& n), n is too large");
                           
        Utils::rsim_assert(this->col_ > n,
                           "Matrix_<T>::extractSquareBlockOfSize(const int& n), n is too large");
        Utils::rsim_assert(n > 0,
                           "Matrix_<T>::extractSquareBlockOfSize(const int& n), n must be greater than zero");
        
        #endif
        Matrix_<T> op(n,n);
        double *mat = op.wPtr();
        for(int i=0;i<n;++i){
                for(int j=0;j<n;++j)
                        mat[n*i+j] = this->data_[i*this->col_+j];
        }
        return op;
}

///////////////////////////////////////////////////////////////

template <class T>
void Matrix_<T>::printDimensions(){
        cout<<"NRow = "<<row_<<"\t"<<"NCol = "<<col_<<endl;
}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T>::Matrix_(const Vector_<T>& V):cData<T>(V),row_(1),col_(1){
        if(V.isColVector())
                row_ = V.getLength();
        else
                col_ = V.getLength();
}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T>::Matrix_():cData<T>(),row_(0),col_(0){}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T>::Matrix_(const Matrix_<T>& M):cData<T>(M),row_(M.getNRow()),col_(M.getNCol()){}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T>::Matrix_(const Matrix_<T>& M, const int& row, const int& col):cData<T>(M,row*col),row_(row),col_(col){}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T>::Matrix_(const int& i,const int& j):cData<T>(i*j),row_(i),col_(j){}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T>::Matrix_(const int& row,
                const int& col,
                const T *dataPtr):cData<T>(row*col,dataPtr),row_(row),col_(col){}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T>::Matrix_(const int& row,const int& col,const T& val):cData<T>(row*col,val),row_(row),col_(col){}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T> Matrix_<T>::operator~(void) const {        
        Matrix_<T> op(col_,row_);
        for(int i=0;i<col_;i++)
                for(int j=0;j<row_;j++)
                        op(i,j)=this->data_[j*col_+i];	
        return op;
}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T> Matrix_<T>::operator-(void) const {        
        #ifdef RSIM_SAFE
        Utils::rsim_assert(row_*col_,"Matrix_<T>::operator-(void), Input matrix is empty");
        #endif
        
        Matrix_<T> op(row_,col_);
        double *optr = op.wPtr();
        for(int i=0; i<row_*col_; ++i){
                optr[i]=-this->data_[i];
        }
        return op;
}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T> Matrix_<T>::operator+(const Matrix_<T>& Mat)const{
        int r = Mat.getNRow();
        int c = Mat.getNCol();

        if( (row_-r) | (col_-c)){
                cout<<"\nMatrix_<T> Matrix_<T>::operator+(const Matrix_<T>& Mat)\n\
                Inconsistent Dimensions in Matrix_<T> addition. exiting...\n";
                exit(0);
        }
        Matrix_<T> op(row_,col_);
        const T *m = Mat.Ptr();
        T *opvec = op.wPtr();
        for(int i=0;i<this->nele_;++i)
                opvec[i]=this->data_[i]+m[i];
        return op; 
        
}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T> Matrix_<T>::operator*(const T& val)const{
        if(!this->nele_){
                cout<<"\nMatrix_<T> Matrix_<T>::operator*(const T& val):\n\
                Empty Matrix_<T> Mat. exiting...\n";
                exit(0);
        }
        Matrix_<T> op(row_,col_);
        
        T *opmat = op.wPtr();
        for(int i=0;i<this->nele_;++i)
                opmat[i]=val*this->data_[i];
        return op; 
}

///////////////////////////////////////////////////////////////

template <class T>
Vector_<T> Matrix_<T>::operator+(const Vector_<T>& V)const{

        bool iscolvec = V.isColVector();
        int len = V.getLength();
        if(iscolvec && (col_==1) && (this->nele_==len)){
                Vector_<T> op(len);
                T *opvec = op.wPtr();
                const T *v = V.Ptr();

                for(int i=0;i<len;++i)
                        opvec[i] = v[i]+this->data_[i];
                return op;
        }
        else if(!iscolvec && (row_ == 1) && (this->nele_==len)){
                
                Vector_<T> op(len);
                op.isColVector() = false;
                T *opvec = op.wPtr();
                const T *v = V.Ptr();
                
                for(int i=0;i<len;++i)
                        opvec[i] = v[i]+this->data_[i];
                return op;
        }
        cout<<"\nVector_<T> Matrix_<T>::operator+(const Vector_<T>& V)const\n\
        Inconsistent Dimensions in addition. exiting...\n";
        exit(0);
}

///////////////////////////////////////////////////////////////

template <class T>
Vector_<T> Matrix_<T>::operator-(const Vector_<T>& V)const{

        bool iscolvec = V.isColVector();
        int len = V.getLength();
        if(iscolvec && (col_==1) && (this->nele_==len)){
                Vector_<T> op(len);
                T *opvec = op.wPtr();
                const T *v = V.Ptr();

                for(int i=0;i<len;++i)
                        opvec[i] = -v[i]+this->data_[i];
                return op;
        }
        else if(!iscolvec && (row_ == 1) && (this->nele_==len)){
                
                Vector_<T> op(len);
                op.isColVector() = false;
                T *opvec = op.wPtr();
                const T *v = V.Ptr();
                
                for(int i=0;i<len;++i)
                        opvec[i] = -v[i]+this->data_[i];
                return op;
        }
        cout<<"\nVector_<T> Matrix_<T>::operator-(const Vector_<T>& V)const\n\
        Inconsistent Dimensions in addition. exiting...\n";
        exit(0);
}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T> Matrix_<T>::operator*(const Matrix_<T>& M)const{
        
        int Brows = M.getNRow();
        int Bcols = M.getNCol();
        if(col_!=Brows){
                cout<<"Matrix_<T> Matrix_<T>::operator*(const Matrix_<T>& M):\n\
                Incompatible Matrix_<T> dimensions. About to exit...\n";
                exit(0);
        }
        Matrix_<T> op(row_,Bcols);
        
        const T *m2 = M.Ptr();
        T *opmat = op.wPtr();
        
        T sum;
        for(int i = 0;i<row_;i++){
                for(int j=0;j<Bcols;j++){
                        sum=0.0;
                        for(int k=0;k<col_;k++)
                                sum = sum+this->data_[i*col_+k]*m2[k*Bcols+j];
                        opmat[i*Bcols+j]=sum;
                }
        }
        return op;
}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T> Matrix_<T>::operator*(const Vector_<T>& V)const{
        int Brows = V.getLength();
        int Bcols = 1;
        if(!V.isColVector()){
                Bcols = Brows;
                Brows = 1;
        }
        if(col_!=Brows){
                cout<<"\nMatrix_<T> Matrix_<T>::operator*(const Vector_<T>& V)const\n\
                Incompatible Matrix_<T>/Vector_<T> dimensions. About to exit...\n";
                exit(0);
        }
        Matrix_<T> op(row_,Bcols);
        const T *m2 = V.Ptr();
        T *opmat = op.wPtr();
        
        T sum;
        for(int i = 0;i<row_;++i){
                for(int j=0;j<Bcols;++j){
                        sum=0.0;
                        for(int k=0;k<col_;++k)
                                sum = sum+this->data_[i*col_+k]*m2[k*Bcols+j];
                        opmat[i*Bcols+j]=sum;
                }
        }
        return op;
}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T> Matrix_<T>::operator-(const Matrix_<T>& Mat)const{
        int r = Mat.getNRow();
        int c = Mat.getNCol();

        if( (row_-r) | (col_-c)){
                cout<<"\nMatrix_<T> Matrix_<T>::operator+(const Matrix_<T>& Mat)\n\
                Inconsistent Dimensions in Matrix_<T> addition. exiting...\n";
                exit(0);
        }
        Matrix_<T> op(row_,col_);
        const T *m = Mat.Ptr();
        T *opvec = op.wPtr();
        for(int i=0;i<this->nele_;++i)
                opvec[i]=this->data_[i]-m[i];
        return op; 
}

///////////////////////////////////////////////////////////////

template <class T>
void Matrix_<T>::operator=(const Matrix_<T>& M){
        int r = M.getNRow();
        int c = M.getNCol();
        if((row_==r) && (col_==c)){
                const T *ptr = M.Ptr();
                for(int i=0;i<this->nele_;++i)
                        this->data_[i] = ptr[i];
        }
        else if(!this->nele_){
                this->setDimension(r,c);
                const T *ptr = M.Ptr();
                for(int i=0;i<this->nele_;++i){
                        this->data_[i] = ptr[i];
                }
        }
        else{
                cout<<"\nFrom Matrix_<T>.h: Matrix_<T> Matrix_<T>::operator=(const Matrix_<T>& M)\n\
                Incompatible Matrix_<T> dimensions. Exiting... \n";
                exit(0);
        }
}

///////////////////////////////////////////////////////////////

template <class T>
void Matrix_<T>::operator=(const Vector_<T>& V){
        bool iscolvec = V.isColVector();
        int c,r,len;
        len = V.getLength();
        if(iscolvec){
                c = 1;
                r = len;
        }
        else{
                r = 1;
                c = len;
        }
        
        if((row_==r) && (col_==c)){
                const T *ptr = V.Ptr();
                for(int i=0;i<this->nele_;++i)
                        this->data_[i] = ptr[i];
        }
        else if(!this->nele_){
                this->setDimension(r,c);
                const T *ptr = V.Ptr();
                for(int i=0;i<this->nele_;++i){
                        this->data_[i] = ptr[i];
                }
        }
        else{
                cout<<"\nFrom Matrix_<T>.h: Matrix_<T> Matrix_<T>::operator=(const Vector_<T>& V)\n\
                Incompatible Matrix_<T> dimensions. Exiting... \n";
                exit(0);
        }
}

///////////////////////////////////////////////////////////////

template <class T>
inline void Matrix_<T>::clear(){this->clear_();row_=0;col_=0;}

///////////////////////////////////////////////////////////////

template <class T>
inline int Matrix_<T>::getNRow()const{return row_;}

///////////////////////////////////////////////////////////////

template <class T>
inline int Matrix_<T>::dim1()const{return row_;}

///////////////////////////////////////////////////////////////

template <class T>
inline int Matrix_<T>::getNCol()const{return col_;}

///////////////////////////////////////////////////////////////

template <class T>
inline int Matrix_<T>::dim2()const{return col_;}

///////////////////////////////////////////////////////////////

template <class T>
inline int Matrix_<T>::getNElements()const{return this->nele_;}


///////////////////////////////////////////////////////////////

template <class T>
void Matrix_<T>::setDimension(const int& NumRow,const int& NumCol){
        row_ = NumRow;
        col_ = NumCol;
        this->setLength(row_*col_);
}

///////////////////////////////////////////////////////////////

template <class T>
void Matrix_<T>::resize(const int& NumRow,const int& NumCol){
        row_ = NumRow;
        col_ = NumCol;
        this->setLength(row_*col_);
}

///////////////////////////////////////////////////////////////

template <class T>
inline const T& Matrix_<T>::getElement(const int& i,const int& j)const{return this->data_[i*col_+j];}

///////////////////////////////////////////////////////////////

template <class T>
inline const T& Matrix_<T>::getElement(const int& i)const{return this->data_[i];}

///////////////////////////////////////////////////////////////

template <class T>
inline void Matrix_<T>::setElement(const int& i,const int& j,const T& v){this->data_[i*col_+j]=v;}

///////////////////////////////////////////////////////////////

template <class T>
inline void Matrix_<T>::setElement(const int& i,const T& v){this->data_[i]=v;}

///////////////////////////////////////////////////////////////

template <class T>
inline void Matrix_<T>::Zeros(){this->Zeros_();}

///////////////////////////////////////////////////////////////

template <class T>
inline void Matrix_<T>::Diagonal(const T& val){Zeros();for(int i=0;i<row_;i++)this->data_[i*col_+i]=val;}

///////////////////////////////////////////////////////////////

template <class T>
inline void Matrix_<T>::Identity(){this->Zeros();this->Diagonal(1.0);}

///////////////////////////////////////////////////////////////

template <class T>
inline T& Matrix_<T>::operator () (const int& i, const int& j){return this->data_[i*col_+j];}

///////////////////////////////////////////////////////////////

template <class T>
inline T& Matrix_<T>::operator () (const int& i){return this->data_[i];}

///////////////////////////////////////////////////////////////

template <class T>
inline T* Matrix_<T>::operator[](const int& i){return this->data_+col_*i;}

///////////////////////////////////////////////////////////////

template <class T>
inline const T* Matrix_<T>::operator[](const int& i)const{return this->data_+col_*i;}

///////////////////////////////////////////////////////////////

template <class T>
std::ostream& operator<<(ostream& func,const Matrix_<T>& M){
        const int& row = M.getNRow();
        const int& col = M.getNCol();	
        for(int i=0;i<row;i++){
                for(int j=0;j<col;++j){
                        if(i==row-1 && j == col-1)
                                func<<M.getElement(i,j); 
                        else                                
                                func<<M.getElement(i,j)<<",";                         
                }
                cout<<endl;
        }
        return func;
}

///////////////////////////////////////////////////////////////

template <class T>
Matrix_<T> operator*(const double& val,const Matrix_<T>& Mat){
        return (Mat*val);
}

///////////////////////////////////////////////////////////////

template <class T>
std::istream& operator>>(istream& func,Matrix_<T>& M){
        const int& row = M.getNRow();
        const int& col = M.getNCol();
        double *ptr = M.wPtr();
        for(int i=0;i<row;i++){
                for(int j=0;j<col;++j)
                        func>>ptr[i*col+j]; 
        }
        return func;
}

///////////////////////////////////////////////////////////////

} // namespace RSIM

#endif
